﻿
// 2020/1/16: 首个记录，基于V3 2019/10/11
// 2020/5/15: 移除编译warning
// 2020/6/17: public ID改为raw ID
// 2020/7/20: 更新样本标题
// 2020/9/29: 修正TTLC计算
// 2020/11/12: 支持Spadas7

#ifndef LANE_SENSOR_SAMPLE_V4_H
#define LANE_SENSOR_SAMPLE_V4_H

#include "spadas.h"

namespace lane_sensor_sample_v4
{
	using namespace spadas;

#if SPADAS_VERSION_MAJOR < 7
	typedef OptionalDouble GeneralElement;
#endif

	enum LineDescription
	{
		LD_Model = 1, // 通过模型参数和一致的属性进行描述
		LD_Segments = 2, // 通过多段线段和各自的属性进行描述
		LD_Both = 3, // Model和Segments双模式描述
	};

	enum LineClass
	{
		LC_General = 1, // 未指定
		LC_Dashed = 2, // 虚线
		LC_Solid = 3, // 实线
		LC_DoubleDashed = 4, //双虚线
		LC_DoubleSolid = 5, //双实线
		LC_SolidDashed = 6, // 左实右虚
		LC_DashedSolid = 7, // 左虚右实
		LC_Curb = 8, // 路沿
		LC_Guardrail = 9, // 护栏
		LC_Barrier = 10, //障碍物边界
	};

	enum LineColor
	{
		LCR_Unknown = 1, // 未知
		LCR_White = 2, // 白色
		LCR_Yellow = 3, // 黄色
		LCR_Red = 4, // 红色
		LCR_Blue = 5, // 蓝色
		LCR_Orange = 6, // 橙色
		LCR_Green = 7, // 绿色
	};

	struct LineSegmentSolidProperty
	{
		Bool valid; // 是否有效，FALSE表示为虚线段
		LineColor color; // 线段颜色
		OptionalDouble width; // 线段宽度 m

		LineSegmentSolidProperty()
		{
			valid = FALSE;
			color = LCR_Unknown;
		}
	};

	struct LineSegment
	{
		Int id; // 线段ID
		Point2D p1; // 线段首端点的x-y坐标 m
		OptionalDouble z1; // 线段首端点的z坐标 m
		Point2D p2; // 线段末端点的x-y坐标 m
		OptionalDouble z2; // 线段末端点的z坐标 m
		LineSegmentSolidProperty solidProperty; // 实线段的属性（虚线段则为空）
	};

	struct LaneLine
	{
		// 基本信息
		Int id; // 车道线ID，0表示无效
		OptionalInt rawID; // 车道线原始ID
		OptionalDouble confidence; // 车道线置信度 %
		LineDescription description; // 车道线的描述方式

		// 模型描述
		LineClass clas; // Model模式：一致性车道线类型
		LineColor color; // Model模式：一致性车道线颜色
		OptionalDouble width; // Model模式：一致性车道线宽度 m
		Double param[4]; // Model模式：y = coef[0] + coef[1] * x + coef[2] * x^2 + coef[3] * x^3
		Bool paramZValid; // Model模式：z方程是否有效
		Double paramZ[4]; // Model模式：z = coef[0] + coef[1] * x + coef[2] * x^2 + coef[3] * x^3
		OptionalDouble rearEnd; // Model模式：曲线后端点位置(x轴) m
		OptionalDouble frontEnd; // Model模式：曲线前端点位置(x轴) m

		// 线段描述
		Array<LineSegment> segments; // Segments模式：线段列表

		LaneLine()
		{
			id = -1;
			description = LD_Model;
			clas = LC_General;
			color = LCR_Unknown;
			param[0] = param[1] = param[2] = param[3] = 0;
			paramZValid = FALSE;
			paramZ[0] = paramZ[1] = paramZ[2] = paramZ[3] = 0;
		}

		Bool modelValid()
		{
			return description == LD_Model || description == LD_Both;
		}

		Bool segmentsValid()
		{
			return description == LD_Segments || description == LD_Both;
		}
	};

	struct LaneSensorSample
	{
		GlobalTimestamp timeStamp;// 时间戳

		// 车道线列表
		Array<LaneLine> lines; // 车道线列表
		OptionalInt firstLeftLine; // 左侧第一条车道线序号
		OptionalInt firstRightLine; // 右侧第一条车道线序号
		OptionalInt secondLeftLine; // 左侧第二条车道线序号
		OptionalInt secondRightLine; // 右侧第二条车道线序号

		// 道路属性部分
		OptionalDouble laneWidth; // 当前车道宽度 m
		OptionalDouble laneHeading; // 当前车道朝向 deg
		OptionalDouble laneCurvature; // 当前车道曲率 1/m

		// 本车道行驶状态
		OptionalDouble lateralVelocity; // 当前车道横向速度 m/s
		OptionalDouble centerDeparture; // 当前车道中央偏离距离 m

		// 参考车辆信息
		OptionalDouble vehiVX; // 本车车速 kph
		OptionalDouble vehiCurv; // 本车行驶曲率 1/m
		OptionalDouble vehiAX; // 本车纵向加速度 m/s²
		OptionalDouble vehiWidth; // 本车宽度 m
		OptionalDouble vehiOverhang; // 本车前悬 m
		OptionalDouble vehiWheelBase; // 本车轴距 m

		// 车道线识别边界
		Double rearBound; // 传感器检测范围后边界 m
		Double frontBound; // 传感器检测范围前边界 m

		LaneSensorSample()
		{
			rearBound = 0;
			frontBound = 30;
		}

		static String getProtocolName(UInt channel)
		{
			if (channel == UINF) return "lane-sensor-sample-v4";
			return (String)"lane-sensor-sample-v4@" + channel;
		}

		static String getTitle()
		{
			return "Line count,Segment count,Lateral velocity[m/s],Center departure[m],Lane width[m],Lane heading[°],Lane curvature[1/m],Rear detect bound[m],Front detect bound[m],First left line index,First right line index,Second left line index,Second right line index,Vehicle speed[KPH],Vehicle curvature[1/m],Vehicle acceleration[m/s2],Vehicle width[m],Vehicle overhang[m],Vehicle wheel base[m],Reserved1,Reserved2,Reserved3,Reserved4,First line's ID,Line's raw ID,Line's confidence,Line's description style,Line type,Line color,Line width,Line's y-formula order-0,Line's y-formula order-1,Line's y-formula order-2,Line's y-formula order-3,Line's z-formula vaild,Line's z-formula order-0,Line's z-formula order-1,Line's z-formula order-2,Line's z-formula order-3,Line's rear end,Line's front end,Line's reserved1,Line's reserved2,Line's reserved3,Line DTLC,Line STLC,Line TTLC,Line's reserved4,Line's segment data offset,Line's segment count,Second line's ID / First segment's ID,etc.";
		}

		static Bool supportInterpolation()
		{
			return FALSE;
		}

		GeneralSample toGeneralSample()
		{
			GeneralSample sample;
			sample.timeStamp = timeStamp;

			UInt segmentCount = 0;
			Array<UInt> segmentOffset(lines.size());
			Array<UInt> segmentSize(lines.size());
			for (UInt i = 0; i < lines.size(); i++)
			{
				segmentOffset[i] = segmentCount;
				segmentSize[i] = lines[i].segments.size();
				segmentCount += segmentSize[i];
			}

			sample.values = Array<GeneralElement>(23 + lines.size() * 27 + segmentCount * 12);

			sample.values[0] = lines.size();
			sample.values[1] = segmentCount;

			sample.values[2] = lateralVelocity;
			sample.values[3] = centerDeparture;
			sample.values[4] = laneWidth;
			sample.values[5] = laneHeading;
			sample.values[6] = laneCurvature;
			sample.values[7] = rearBound;
			sample.values[8] = frontBound;
			sample.values[9] = firstLeftLine.valid ? firstLeftLine.value : OptionalDouble();
			sample.values[10] = firstRightLine.valid ? firstRightLine.value : OptionalDouble();
			sample.values[11] = secondLeftLine.valid ? secondLeftLine.value : OptionalDouble();
			sample.values[12] = secondRightLine.valid ? secondRightLine.value : OptionalDouble();
			sample.values[13] = vehiVX;
			sample.values[14] = vehiCurv;
			sample.values[15] = vehiAX;
			sample.values[16] = vehiWidth;
			sample.values[17] = vehiOverhang;
			sample.values[18] = vehiWheelBase;

			UInt segBase = 23 + lines.size() * 27;

			for (UInt i = 0; i < lines.size(); i++)
			{
				LaneLine& line = lines[i];
				UInt baseIndex = 23 + i * 27;

				sample.values[baseIndex + 0] = (Double)line.id;
				sample.values[baseIndex + 1] = line.rawID.valid ? (Double)line.rawID.value : OptionalDouble();
				sample.values[baseIndex + 2] = line.confidence;
				sample.values[baseIndex + 3] = (Double)(Int)line.description;

				if (line.modelValid())
				{
					sample.values[baseIndex + 4] = (Double)(Int)line.clas;
					sample.values[baseIndex + 5] = (Double)(Int)line.color;
					sample.values[baseIndex + 6] = line.width;
					sample.values[baseIndex + 7] = line.param[0];
					sample.values[baseIndex + 8] = line.param[1];
					sample.values[baseIndex + 9] = line.param[2];
					sample.values[baseIndex + 10] = line.param[3];
					sample.values[baseIndex + 11] = line.paramZValid ? 1.0 : 0.0;
					sample.values[baseIndex + 12] = line.paramZ[0];
					sample.values[baseIndex + 13] = line.paramZ[1];
					sample.values[baseIndex + 14] = line.paramZ[2];
					sample.values[baseIndex + 15] = line.paramZ[3];
					sample.values[baseIndex + 16] = line.rearEnd;
					sample.values[baseIndex + 17] = line.frontEnd;
				}

				if (line.segmentsValid())
				{
					UInt segOffset = segmentOffset[i];
					UInt segSize = segmentSize[i];
					sample.values[baseIndex + 25] = (Double)segOffset;
					sample.values[baseIndex + 26] = (Double)segSize;
					
					if (segSize > 0)
					{
						for (UInt n = 0; n < segSize; n++)
						{
							LineSegment& seg = line.segments[n];
							UInt segBaseIndex = segBase + 12 * (segOffset + n);

							sample.values[segBaseIndex + 0] = (Double)seg.id;
							sample.values[segBaseIndex + 1] = seg.p1.x;
							sample.values[segBaseIndex + 2] = seg.p1.y;
							sample.values[segBaseIndex + 3] = seg.z1;
							sample.values[segBaseIndex + 4] = seg.p2.x;
							sample.values[segBaseIndex + 5] = seg.p2.y;
							sample.values[segBaseIndex + 6] = seg.z2;
							sample.values[segBaseIndex + 7] = seg.solidProperty.valid ? 1.0 : 0.0;
							sample.values[segBaseIndex + 8] = (Double)(Int)seg.solidProperty.color;
							sample.values[segBaseIndex + 9] = seg.solidProperty.width;
						}
					}
				}

				sample.values[baseIndex + 21] = getDTLC(line);
				sample.values[baseIndex + 22] = getSTLC(line);
				sample.values[baseIndex + 23] = getTTLC(line);
			}

			sample.significantCount = sample.values.size();
			return sample;
		}

		Bool fromGeneralSample(String protocol, GeneralSample sample)
		{
			if (protocol == "lane-sensor-sample-v4")
			{
				Array<GeneralElement> v = sample.values;

				if (v.size() < 23) return FALSE;
				if (!v[0].valid || !v[1].valid) return FALSE;

				Int lineCount = (Int)v[0].value;
				Int segmentCount = (Int)v[1].value;
				if ((Int)v.size() != 23 + lineCount * 27 + segmentCount * 12) return FALSE;

				timeStamp = sample.timeStamp;

				lateralVelocity = v[2];
				centerDeparture = v[3];
				laneWidth = v[4];
				laneHeading = v[5];
				laneCurvature = v[6];
				rearBound = v[7].valid ? v[7].value : 0;
				frontBound = v[8].valid ? v[8].value : 30;
				firstLeftLine = v[9].valid ? (Int)v[9].value : OptionalInt();
				firstRightLine = v[10].valid ? (Int)v[10].value : OptionalInt();
				secondLeftLine = v[11].valid ? (Int)v[11].value : OptionalInt();
				secondRightLine = v[12].valid ? (Int)v[12].value : OptionalInt();
				vehiVX = v[13];
				vehiCurv = v[14];
				vehiAX = v[15];
				vehiWidth = v[16];
				vehiOverhang = v[17];
				vehiWheelBase = v[18];

				Int segBase = 23 + lineCount * 27;

				lines = Array<LaneLine>(lineCount);
				for (int i = 0; i < lineCount; i++)
				{
					LaneLine& line = lines[i];
					UInt baseIndex = 23 + 27 * i;

					line.id = (Int)v[baseIndex + 0].value;
					line.rawID = v[baseIndex + 1].valid ? (Int)v[baseIndex + 1].value : OptionalInt();
					line.confidence = v[baseIndex + 2];
					line.description = (LineDescription)(Int)v[baseIndex + 3].value;

					if (line.modelValid())
					{
						line.clas = (LineClass)(Int)v[baseIndex + 4].value;
						line.color = (LineColor)(Int)v[baseIndex + 5].value;
						line.width = v[baseIndex + 6];
						line.param[0] = v[baseIndex + 7].value;
						line.param[1] = v[baseIndex + 8].value;
						line.param[2] = v[baseIndex + 9].value;
						line.param[3] = v[baseIndex + 10].value;
						line.paramZValid = v[baseIndex + 11].value == 1;
						if (line.paramZValid)
						{
							line.paramZ[0] = v[baseIndex + 12].value;
							line.paramZ[1] = v[baseIndex + 13].value;
							line.paramZ[2] = v[baseIndex + 14].value;
							line.paramZ[3] = v[baseIndex + 15].value;
						}
						line.rearEnd = v[baseIndex + 16];
						line.frontEnd = v[baseIndex + 17];
					}

					if (line.segmentsValid())
					{
						Int segOffset = (Int)v[baseIndex + 25].value;
						Int segSize = (Int)v[baseIndex + 26].value;
						if (segSize > 0)
						{
							line.segments = Array<LineSegment>(segSize);
							for (Int n = 0; n < segSize; n++)
							{
								LineSegment& seg = line.segments[n];
								Int segBaseIndex = segBase + 12 * (segOffset + n);

								seg.id = (Int)v[segBaseIndex + 0].value;
								seg.p1.x = (Float)v[segBaseIndex + 1].value;
								seg.p1.y = (Float)v[segBaseIndex + 2].value;
								seg.z1 = v[segBaseIndex + 3];
								seg.p2.x = (Float)v[segBaseIndex + 4].value;
								seg.p2.y = (Float)v[segBaseIndex + 5].value;
								seg.z2 = v[segBaseIndex + 6];
								seg.solidProperty.valid = v[segBaseIndex + 7].value == 1;
								if (seg.solidProperty.valid)
								{
									seg.solidProperty.color = (LineColor)(Int)v[segBaseIndex + 8].value;
									seg.solidProperty.width = v[segBaseIndex + 9];
								}
							}
						}
					}
				}

				return TRUE;
			}
			else if (protocol == "lane-sensor-sample-v3")
			{
				Array<GeneralElement> v = sample.values;

				if (v.size() < 17) return FALSE;
				if (!v[0].valid) return FALSE;

				int lineCount = (int)v[0].value;
				if ((Int)v.size() != 17 + lineCount * 15) return FALSE;

				timeStamp = sample.timeStamp;

				lateralVelocity = v[1];
				centerDeparture = v[2];
				laneWidth = v[3];
				laneHeading = v[4];
				laneCurvature = v[5];
				vehiVX = v[6];
				vehiCurv = v[7];
				vehiAX = v[8];
				vehiWidth = v[9];
				vehiOverhang = v[10];
				rearBound = v[11].valid ? v[11].value : 0;
				frontBound = v[12].valid ? v[12].value : 30;
				firstLeftLine = v[13].valid ? (Int)v[13].value : OptionalInt();
				firstRightLine = v[14].valid ? (Int)v[14].value : OptionalInt();
				secondLeftLine = v[15].valid ? (Int)v[15].value : OptionalInt();
				secondRightLine = v[16].valid ? (Int)v[16].value : OptionalInt();

				lines = Array<LaneLine>(lineCount);
				for (int i = 0; i < lineCount; i++)
				{
					LaneLine& line = lines[i];
					UInt baseIndex = 17 + 15 * i;

					line.id = (Int)v[baseIndex + 0].value;
					line.rawID = v[baseIndex + 1].valid ? (Int)v[baseIndex + 1].value : OptionalInt();
					line.clas = (LineClass)(Int)v[baseIndex + 2].value;
					line.color = (LineColor)(Int)v[baseIndex + 3].value;
					line.confidence = v[baseIndex + 4];
					line.width = v[baseIndex + 5];
					line.param[0] = v[baseIndex + 6].value;
					line.param[1] = v[baseIndex + 7].value;
					line.param[2] = v[baseIndex + 8].value;
					line.param[3] = v[baseIndex + 9].value;
					line.rearEnd = v[baseIndex + 10];
					line.frontEnd = v[baseIndex + 11];
				}

				return TRUE;
			}
			else return FALSE;
		}

		static LaneSensorSample interpolate(LaneSensorSample& s1, Double w1, LaneSensorSample& s2, Double w2, GlobalTimestamp time)
		{
			return LaneSensorSample();
		}

		OptionalDouble getDTLC(LaneLine& line)
		{
			if (line.modelValid() && vehiWidth.valid && vehiOverhang.valid)
			{
				Double *values = line.param;
				Bool isLeft = values[0] > 0;

				Double x = -1 * vehiOverhang.value;
				Double y = (values[3] * x * x * x + values[2] * x * x
					+ values[1] * x + values[0]) * (isLeft ? 1 : -1);
				Double cosTheta = math::abs(math::cos(math::atan(values[1])));
				return cosTheta * (y - vehiWidth.value / 2.0);
			}
			else return OptionalDouble();
		}

		OptionalDouble getSTLC(LaneLine& line)
		{
			if (line.modelValid() && vehiVX.valid)
			{
				Double *values = line.param;
				Bool isLeft = values[0] > 0;

				Double curv = vehiCurv.valid ? vehiCurv.value : 0;
				Double fo = vehiOverhang.valid ? vehiOverhang.value : 0.9;
				Double wb = vehiWheelBase.valid ? vehiWheelBase.value : 2.8;

				Double sinTheta = math::sin(math::atan(values[1] - curv * (fo + wb)));
				return (isLeft ? -1.0 : 1.0) * sinTheta * vehiVX.value / 3.6;
			}
			else return OptionalDouble();
		}

		OptionalDouble getTTLC(LaneLine& line)
		{
			if (line.modelValid() && vehiVX.valid && vehiWidth.valid && vehiOverhang.valid)
			{
				if (vehiVX.value <= 0) return OptionalDouble();

				Double *values = line.param;
				Bool isLeft = values[0] > 0;

				Double wb = vehiWheelBase.valid ? vehiWheelBase.value : 2.8;
				Double x0 = -vehiOverhang.value - wb;
				Double x = x0;
				Double y = 0;
				Double vx = vehiVX.value / 3.6;

				UInt loopTimes = vx < 5 ? 1000 : (UInt)(50.0 / vx * 100); // 超过50米越线则不输出（精度无法保证）

				Double rx = wb;
				Double ry = (isLeft ? 0.5 : -0.5) * vehiWidth.value;

				for (UInt i = 0; i <= loopTimes; i++)
				{
					Double k = 0;
					if (vehiCurv.valid) k = vehiCurv.value * (x - x0);

					Double cosa = 1.0 / math::abs(1 + k);
					Double sina = cosa * k;

					Double tx = x + cosa * rx - sina * ry;
					Double ty = y + sina * rx + cosa * ry;
					Double ly = values[3] * tx * tx * tx + values[2] * tx * tx + values[1] * tx + values[0];

					Bool passed = FALSE;
					if (isLeft)
					{
						if (ly <= ty) passed = TRUE;
					}
					else
					{
						if (ly >= ty) passed = TRUE;
					}
					if (passed)
					{
						if (i == 0) return OptionalDouble();
						else return (Double)i / 100;
					}

					if (vehiAX.valid) vx += vehiAX.value * 0.01;
					if (vx <= 0) break;

					x += vx * 0.01 * cosa;
					y += vx * 0.01 * sina;
				}

				return OptionalDouble();
			}
			else return OptionalDouble();
		}
	};
}

#endif
